#include "motion_plan.h"
#include "math.h"

MotionPlan::MotionPlan()
{
	memset(&dotTrackingParam, 0, sizeof(TrackingPlanParam));
	dotTrackingParam.TrackNum = 0;
	dotTrackingParam.FirstDotAngle = 0;
	dotTrackingParam.eta = 0.1;
	dotTrackingParam.switchlen = 20;
	dotTrackingParam.maxSpeed = 2.5;
	dotTrackingParam.deltat = 0.1;

	WaitTime = 0;
	WaitTimeNum = 0;
	WaitFlag = 0;

	globalFirstFlag = true;

	memset(&tracking_task, 0, sizeof(anchor_mission_control));
	memset(&actionModelData, 0, sizeof(flash_model_data));

	memset(&actionActuatorControl, 0, sizeof(actuator_control));
	memset(&actionHeadvelControl, 0, sizeof(actuator_control));
	memset(&actionTrajectoryMissionControl, 0, sizeof(trajectory_mission_control));

	memset(&trackline, 0, sizeof(trackline));
	memset(&lineunit, 0, sizeof(lineunit));
}

void MotionPlan::TrajecotryGet(TransState boatstate, anchor_mission_control task)
{
	memset(&dotTrackingParam, 0, sizeof(TrackingPlanParam));

	double nowpos[2] = {0}, homepos[2] = {0}, lengthangle[2] = {0};
	homepos[0] = boatstate.HomePos[0];
	homepos[1] = boatstate.HomePos[1];
	unsigned int seq_num = 0;
	unsigned int precommand = 0;
	for (int i = 0; i < task.mission_num; i++)
	{
		precommand = task.mission[i].command;

		if ((precommand == 16))
		{
			_precmd = precommand;
			tracking_task.mission[seq_num].command = task.mission[i].command;
			tracking_task.mission[seq_num].seq = seq_num;
			tracking_task.mission[seq_num].lat = task.mission[i].lat;//lengthangle[0] * sinf(lengthangle[1] * M_PI / 180.0) / 100.0; // trans latitute to posy
			tracking_task.mission[seq_num].lon = task.mission[i].lon;//lengthangle[0] * cosf(lengthangle[1] * M_PI / 180.0) / 100.0; // trans longitute to posx
			tracking_task.mission[seq_num].alt = task.mission[i].alt;
			tracking_task.mission[seq_num].param1 = task.mission[i].param1;
			tracking_task.mission[seq_num].param2 = task.mission[i].param2;
			tracking_task.mission[seq_num].param3 = task.mission[i].param3;
			tracking_task.mission[seq_num].param4 = task.mission[i].param4;
			printf("trajectory seq: %d, x: %f, y:%f \n", tracking_task.mission[seq_num].seq, tracking_task.mission[seq_num].lon, tracking_task.mission[seq_num].lat);
			seq_num++;
		}
	}
	tracking_task.mission_num = seq_num;
	trackline[0][0] = 0;
	trackline[0][1] = 0;

	dotTrackingParam.DotNum = tracking_task.mission_num;
	dotTrackingParam.TrackNum = 0;
	WaitTimeNum = 0;
	WaitTime = tracking_task.mission[0].param1;
	WaitFlag = 0;

	dotTrackingParam.setVel = tracking_task.mission[0].param3;
	if (dotTrackingParam.setVel < 0)
	{
		dotTrackingParam.setVel = 0;
	}
	dotTrackingParam.ReturnFlag = 0;
	dotTrackingParam.TrackFlag = 0;
	dotTrackingParam.FirstFlag = 0;
	dotTrackingParam.eta = 0.1;
	dotTrackingParam.switchlen = 50;
	dotTrackingParam.maxSpeed = 20;
	dotTrackingParam.deltat = 0.1;

	for (int i = 1; i < dotTrackingParam.DotNum; i++)
	{
		float errpos[2] = {0};
		errpos[0] = tracking_task.mission[i].lat - tracking_task.mission[i - 1].lat;
		errpos[1] = tracking_task.mission[i].lon - tracking_task.mission[i - 1].lon;
		float normerrpos = sqrtf(errpos[0] * errpos[0] + errpos[1] * errpos[1]);
		dotTrackingParam.TrajectoryUnit[i - 1][0] = errpos[0] / normerrpos;
		dotTrackingParam.TrajectoryUnit[i - 1][1] = errpos[1] / normerrpos;
	}
	dotTrackingParam.TargetPos[0] = tracking_task.mission[0].lat;
	dotTrackingParam.TargetPos[1] = tracking_task.mission[0].lon;
}

void MotionPlan::TrajectoryTrackingDirect(TransState boatstate)
{
		float lengthangle[2] = {0}, vel = 0;
		float pos[2]={0};
		float errpos[2] = {0};
		float normerrpos = 0;
		float errfirstphi = 0;
		float anglevel[2]={0};
		float tarangle = 0;
		pos[0] = boatstate.PostoHome[0];
		pos[1] = boatstate.PostoHome[1];

		if(dotTrackingParam.TrackFlag == 0)
		{
				errpos[0] = dotTrackingParam.TargetPos[0] - pos[0];
				errpos[1] = dotTrackingParam.TargetPos[1] - pos[1];
				normerrpos = sqrtf(powf(errpos[0], 2) + powf(errpos[1], 2));
				tarangle = atan2(errpos[0], errpos[1]) * 180.0/M_PI;
				if (tarangle < 0)
				{
						tarangle += 360;
				}
				if(dotTrackingParam.FirstFlag == 0)
				{
						dotTrackingParam.FirstFlag = 1;
						dotTrackingParam.firstphi = tarangle;
				}
				if(normerrpos<=5)
				{
					dotTrackingParam.TrackNum++;
						dotTrackingParam.TrackFlag = 1;
						WaitFlag = 1;
						WaitTime = tracking_task.mission[dotTrackingParam.TrackNum-1].param1;
						if(dotTrackingParam.TrackNum < dotTrackingParam.DotNum)
						{
								dotTrackingParam.setVel = tracking_task.mission[dotTrackingParam.TrackNum].param3;
								if(dotTrackingParam.setVel < 1) {dotTrackingParam.setVel = 1;}
						}
						else
						{
								anglevel[0] = 0;
								anglevel[1] = 0;
								dotTrackingParam.ReturnFlag = 1;
								dotTrackingParam.TrackNum = 0xff; //-----------------------------------------
						}
				}
				else if (normerrpos<=20)
				{
					errfirstphi = tarangle - dotTrackingParam.firstphi;
					if(errfirstphi > 180)
					{
							errfirstphi -= 360;
					}
					else if(errfirstphi < -180)
					{
							errfirstphi += 360;
					}
					if((errfirstphi > 90)||(errfirstphi < -90))
					{
						dotTrackingParam.TrackNum++;
						dotTrackingParam.TrackFlag = 1;
						WaitFlag = 1;
						WaitTime = tracking_task.mission[dotTrackingParam.TrackNum-1].param1;
						if(dotTrackingParam.TrackNum < dotTrackingParam.DotNum)
						{
								dotTrackingParam.setVel = tracking_task.mission[dotTrackingParam.TrackNum].param3;
								if(dotTrackingParam.setVel < 1) {dotTrackingParam.setVel = 1;}
						}
						else
						{
								anglevel[0] = 0;
								anglevel[1] = 0;
								dotTrackingParam.ReturnFlag = 1;
								dotTrackingParam.TrackNum = 0xff; //-----------------------------------------
						}
					}
				}

				vel = dotTrackingParam.setVel;
				if (vel < 0)
						vel = 0;
				if (vel >= dotTrackingParam.setVel)
						vel = dotTrackingParam.setVel;
				anglevel[0] = tarangle;
				anglevel[1] = vel;
		}
		else if((dotTrackingParam.TrackFlag == 1)&&(dotTrackingParam.ReturnFlag == 0))
		{
				// float line[2][2] = {0};
				// float pretovel[2] = {0};
				// float tardot[2] = {0},tardot2[2] = {0}, unit[2] = {0};
				// // line[0][0] = tracking_task.mission[dotTrackingParam.TrackNum-1].lat;
				// // line[0][1] = tracking_task.mission[dotTrackingParam.TrackNum-1].lon;
				// // line[1][0] = tracking_task.mission[dotTrackingParam.TrackNum].lat;
				// // line[1][1] = tracking_task.mission[dotTrackingParam.TrackNum].lon;
				// // Dot2Line(pos, line, tardot, unit);
				// // tardot2[0] = tardot[0] + unit[0] * 150;
				// // tardot2[1] = tardot[1] + unit[1] * 150;
				// tardot2[0] = tardot[0] + unit[0] * 150;
				// tardot2[1] = tardot[1] + unit[1] * 150;

				errpos[0] = tracking_task.mission[dotTrackingParam.TrackNum].lat - pos[0];
				errpos[1] = tracking_task.mission[dotTrackingParam.TrackNum].lon - pos[1];

				normerrpos = sqrtf(errpos[0]*errpos[0] + errpos[1]*errpos[1]);
				if (normerrpos <= dotTrackingParam.switchlen)
				{
						dotTrackingParam.TrackNum++;
						WaitFlag = 1;
						WaitTime = tracking_task.mission[dotTrackingParam.TrackNum-1].param1;
						if(dotTrackingParam.TrackNum < (dotTrackingParam.DotNum))
						{
								dotTrackingParam.setVel = tracking_task.mission[dotTrackingParam.TrackNum].param3;
								if(dotTrackingParam.setVel < 1) {dotTrackingParam.setVel = 1;}
						}
						else
						{
								anglevel[0] = 0;
								anglevel[1] = 0;
								dotTrackingParam.ReturnFlag = 1;
								dotTrackingParam.TrackNum = 0xff; //-----------------------------------------
						}
				}
				dotTrackingParam.TargetPos[0] = tracking_task.mission[dotTrackingParam.TrackNum].lat;
				dotTrackingParam.TargetPos[1] = tracking_task.mission[dotTrackingParam.TrackNum].lon;

				errpos[0] = (dotTrackingParam.TargetPos[0] - pos[0]) * dotTrackingParam.eta;
				errpos[1] = (dotTrackingParam.TargetPos[1] - pos[1]) * dotTrackingParam.eta;
				normerrpos = sqrtf(powf(errpos[0], 2) + powf(errpos[1], 2));
				tarangle = atan2(errpos[0], errpos[1]) * 180.0/M_PI;
				if (tarangle < 0)
				{
						tarangle += 360;
				}
				//vel = normerrpos*dotTrackingParam.eta;
				vel = dotTrackingParam.setVel;
				if (vel < 0)
						vel = 0;
				if (vel >= dotTrackingParam.setVel)
						vel = dotTrackingParam.setVel;
				anglevel[0] = tarangle;
				anglevel[1] = vel;
		}
		else if(dotTrackingParam.ReturnFlag == 1)
		{
			printf("finish!!!  direct control stop tracking  33333!\n");
			anglevel[0] = 0;
			anglevel[1] = 0;
		}
		// printf("angle:%f, vel:%f,\n", anglevel[0], anglevel[1]);
	actionModelData.model = 10;
	actionHeadvelControl.model = 2;
	actionHeadvelControl.heading = anglevel[0];
	actionHeadvelControl.velocity = anglevel[1];
}


